from math import *
from numpy import *
from numpy.linalg import *
from copy import deepcopy
class path_finder(object):
	def __init__(self):
		None
	def find_path(self, graph, start, finish, heuristic=lambda x,y: float(norm(array([x[0]-y[0], x[1]-y[1]])))):
		"""Takes in a graph, then a starting and ending position on that graph, and finds the optimal path from start to finish using A* pathfinding. The default heuristic function is the Euclidean distance between two points. This function assumes that there is a path between start and finish; if there is not such a path, it will loop infinitely."""
		indices=graph.keys()
		for item in indices:
			print indices.index(item)
			print item
		path_queue = []
		path=[[start], 0, heuristic(start, finish)]
		while path[0][-1] != finish:
			#print "Current path: "+str(path) #Debug output
			#print "Current node: "+str(path[0][-1]) #Debug output
			for dest_node in graph[path[0][-1]]:
				have_seen = False
				for node in path[0]:
					if dest_node == node:
						have_seen = True
						break
				if dest_node != path[0][-1] and not(have_seen): #if the destination is not the current node, and we have not already been to it
					tmp_path = deepcopy(path)
					cost = heuristic(path[0][-1], dest_node)
					tmp_path[0].append(dest_node)
					tmp_path[1] += cost
					tmp_path[2] = heuristic(dest_node, finish)
					print "Source node: " + str(indices.index(path[0][-1]))
					print "Destination node: " + str(indices.index(dest_node))
					#print "Temp path: "+str(tmp_path) #Debug output
					path_queue.append(tmp_path)
			path_queue.sort(self.prioritize)
			path = path_queue.pop(0)
		#print "Final path: "+str(path) #Debug output
		return path[0]
	@staticmethod
	def prioritize(x,y):
		"""A priority function that is used to sort the priority queue of paths in find_path. Compares F = G + H for both paths, to determine which has the lower cost."""
		return int((x[1]+x[2])-(y[1]+y[2]))